//
// Created by 13562 on 2024/2/3.
//

#ifndef SRC_IMUHANDLER_H
#define SRC_IMUHANDLER_H

#include "sensor_msgs/Imu.h"
#include "std_msgs/Header.h"
#include <queue>
#include <thread>
#include <string>
#include <fstream>
#include <sstream>

typedef enum { ts, acc_x, acc_y, acc_z, ang_x, ang_y, ang_z, ori_x, ori_y, ori_z, ori_w } DataSeries;

typedef struct {
	std::string frame_id;
	std::vector<DataSeries> data_series;
	std::vector<double> series_default;
	unsigned long seq_start;
	unsigned long seq_end;
} MsgInfo;

class ImuHandler {
private:
	std::string _file;
	MsgInfo _msgInfo;

	bool _halt = false;

	std::queue<sensor_msgs::Imu> _msgQueue;

	void _handle();

	unsigned long _count;

public:
	ImuHandler() = default;

	void handle(const std::string& file, const MsgInfo& msgInfo);

	unsigned long getQueueLength();

	void halt();

	sensor_msgs::Imu popMsg();

	bool isReadingFile();

	unsigned long count();
};


#endif //SRC_IMUHANDLER_H
